
#pragma warning (disable : 4251 4996 4819 4244)
#include <new>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <boost/shared_array.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <libplayerc++/playerc++.h>
#include "MapProxyStateValidityChecker.h"
#include "StateRecorder.h"
#include "Order.h"
#include "Executor.h"
#include "PlayerObjectManager.h"

using namespace PlayerCc;
using namespace ompl::base;
using namespace ompl::geometric;


static const double MAX_PLAN_TIME = 0.01;

class Controller {

  boost::random::random_device rng;
  boost::random::uniform_int_distribution<> coin;

  // player parameters
  PlayerClient    robot;
  SimulationProxy sim;
  MapProxy        map;
  Position2dProxy pp;
  Position2dProxy gocon;
  Graphics2dProxy g;

  // ompl parameters
  StateSpacePtr space;
  SimpleSetup ss;

  // standby pos is robot start
  double standbyX;
  double standbyY;

  // tables
  const std::vector<PlayerObject> & tables;

  // order simulator
  std::auto_ptr<customer::OrderSimulator> ordersim;

  double reachDist;

public:

  Controller() :
    coin(0,1),
    robot("localhost"),
    sim(&robot, 0),
    map(&robot, 0),
    pp(&robot, 0),
    gocon(&robot, 100),
    g(&robot, 0),
    space(new SE2StateSpace()),
    ss(space),
    tables(PlayerObjectManager::Instance().GetStaticObjects()),
    reachDist(0.5)
  {
    robot.Read();
    map.RequestMap();

    // TODO: set bounds w.r.t map
    std::cout << "TODO: set bounds w.r.t. map\n";
    RealVectorBounds bounds(2);
    bounds.setLow(0, -10); bounds.setHigh(0, 10);
    bounds.setLow(1, -5); bounds.setHigh(1, 5);
    space->as<SE2StateSpace>()->setBounds(bounds);

    // set validity checker
    StateValidityCheckerPtr checker(new MapProxyStateValidityChecker(0.4, ss.getSpaceInformation(), map));
    ss.setStateValidityChecker(checker);

    // standby position is start pos
    standbyX = pp.GetXPos();
    standbyY = pp.GetYPos();

    // get tables and init order sim
    ordersim.reset(new customer::OrderSimulator(tables.size()));
  }

private:

  PlannerPtr getPlanner() {
    PlannerPtr planner(new RRT(ss.getSpaceInformation()));
    planner->as<RRT>()->setRange(0.5);
    return planner;
  }

  void standby() {
    // get current robot positions etc.
    robot.Read();

    // start at the existing position
    ScopedState<SE2StateSpace> start(ss.getSpaceInformation());
    start->setX(pp.GetXPos()); start->setY(pp.GetYPos());

    // goal is the standby position
    ScopedState<SE2StateSpace> goal(ss.getSpaceInformation());
    goal->setX(standbyX); goal->setY(standbyY);

    // TODO: check distance between start, goal

    std::cout << "Going to Standby" << std::endl;

    // set start and goal
    ss.setStartAndGoalStates(start, goal);

    // setup the path planner
    ss.setPlanner(getPlanner());
    ss.clear();
    ss.setup();
    //ss.print(std::cout);

    // plan
    PlannerStatus solved = ss.solve();

    record_state( ss.getSolutionPath(), "StandbyPath.csv" );

    PlannerData oPlannerData( ss.getSpaceInformation() );
    ss.getPlanner()->getPlannerData( oPlannerData );

    std::vector<State*> vecStates;
    for ( unsigned int nLoop = 0; nLoop < oPlannerData.numVertices(); ++nLoop )
    {
        vecStates.push_back((State*)(oPlannerData.getVertex( nLoop ).getState()));
    }
    record_state( vecStates, "Standby.plantree.csv" );

    // execute
    if (solved) {
       ss.simplifySolution();
       Executor executor(ss.getSolutionPath(), robot, pp, gocon);
       executor.execute();
    } else {
       std::cout << "Unable to go to standby pos!" << std::endl;
    }

  }

  // generate a goal for a particular table, not necessary valid
  std::pair<double,double> generateGoal(const PlayerObject & table) {
    double x, y;

    // generate X
    if (coin(rng)) {
      boost::uniform_real<> u(table.PosLowX - reachDist, table.PosLowX);
      x = u(rng);
    } else {
      boost::uniform_real<> u(table.PosHighX, table.PosHighX + reachDist);
      x = u(rng);
    }

    // generate Y
    if (coin(rng)) {
      boost::uniform_real<> u(table.PosLowY - reachDist, table.PosLowY);
      y = u(rng);
    } else {
      boost::uniform_real<> u(table.PosHighY, table.PosHighY + reachDist);
      y = u(rng);
    }

    return std::make_pair(x,y);
  }

  // generates a valid goal position
  std::pair<double,double> generateValidGoal(int tableIndex) {
    const PlayerObject & table = tables[tableIndex];
    for (int i = 0; i < 100; ++i) {
      std::pair<double,double> gg = generateGoal(table);
      // check state valid
      ScopedState<SE2StateSpace> cg(ss.getSpaceInformation());
      cg->setX(gg.first); cg->setY(gg.second);
      if (ss.getStateValidityChecker()->isValid(cg.get())) {
        return gg;
      }
    }
    throw std::runtime_error("no valid goal position!");
  }

  void deliver(const std::vector<customer::Order> orders) {

    int deliverSize = orders.size();
    int deliverNum = 1;

    std::cout << "Delivering " << deliverSize << " orders" << std::endl;

    for (std::vector<customer::Order>::const_iterator i = orders.begin();
      i != orders.end(); ++i) {

      std::string tableName = tables[i->table].Name;

      std::cout << "Delivering #" << deliverNum << "/" << deliverSize << " to "<< i->table << " (" << tableName.c_str() << ")" << std::endl;

      // get current robot positions etc.
      robot.Read();

      // start at the existing position
      ScopedState<SE2StateSpace> start(ss.getSpaceInformation());
      start->setX(pp.GetXPos()); start->setY(pp.GetYPos());

      // goal is at near table position
      ScopedState<SE2StateSpace> goal(ss.getSpaceInformation());
      std::pair<double,double> nearTable = generateValidGoal(i->table);
      goal->setX(nearTable.first); goal->setY(nearTable.second);

      std::cout << "-> goal (" << nearTable.first << ", " << nearTable.second << ")" << std::endl;

      // set start and goal
      ss.setStartAndGoalStates(start, goal);

      // setup path planner
      ss.setPlanner(getPlanner());
      ss.clear();
      ss.setup();
      //ss.print(std::cout);

      // plan
      PlannerStatus solved = ss.solve();

      std::stringstream stmFile;
      stmFile << "Solution" << i->table << ".csv"; 

      record_state( ss.getSolutionPath(), stmFile.str() );

      PlannerData oPlannerData( ss.getSpaceInformation() );
      ss.getPlanner()->getPlannerData( oPlannerData );

      std::vector<State*> vecStates;
      for ( unsigned int nLoop = 0; nLoop < oPlannerData.numVertices(); ++nLoop )
      {
          vecStates.push_back((State*)(oPlannerData.getVertex( nLoop ).getState()));
      }
      record_state( vecStates, stmFile.str() + ".plantree.csv" );

      // execute
      if (solved) {
        ss.simplifySolution();
        Executor executor(ss.getSolutionPath(), robot, pp, gocon);
        executor.execute();
      } else {
      }

      std::cout << "Delivery #" << deliverNum++ << "/" << deliverSize << " to " << i->table << " (" << tableName.c_str() << ") " << (solved? "successful" : "failed") << std::endl;
    }
  }

public:

  // control loop
  void run() {

    for (;;) {
      // generate a list of orders to fulfil
      std::vector<customer::Order> orders = ordersim->getOrders();

      // if no orders then standby, else deliver them
      if (!orders.empty()) {
        deliver(orders);
        standby();
      }
    }

  }
};

int
main(int argc, char *argv[])
{
    std::vector<std::string> vecDefFiles;
    vecDefFiles.push_back( "../worlds/tables.inc" );
    vecDefFiles.push_back( "../worlds/person.inc" );
    PlayerObjectManager::Instance().ParseConfigFile( vecDefFiles, "../worlds/simple.world" );
  try {
    Controller control;
    control.run();
    return 0;
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
